#include <ros/ros.h>
#include <iostream>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <sstream>

#include "prometheus_msgs/DroneState.h"
#include "prometheus_msgs/ControlCommand.h"
#include "printf_utils.h"
#include "mission_utils.h"
// 自定义的msg，用来画曲线
#include "visual_servo_landing/MyPlot.h"

using namespace std;
// using namespace Eigen;
#define NODE_NAME "my_landing"

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
bool g_use_pad_height; // 是否使用降落板绝对高度
float g_pad_height;
int g_uav_id;
float max_height; // 起始降落位置
float g_camera_offset[3];
prometheus_msgs::ControlCommand g_command_now;
std::string curr_mode;

// 消息打印
std::stringstream ss;

visual_servo_landing::MyPlot plot_msg;   // 作图的自定义消息

//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState g_UAVState; // 无人机状态
Eigen::Matrix3f R_Body_to_ENU;      // 无人机机体系至惯性系旋转矩阵
Eigen::Matrix3f R_c2v;      // 无人机相机坐标系到虚拟相机坐标系的旋转矩阵

//---------------------------------------Vision---------------------------------------------
nav_msgs::Odometry g_GroundTruth;   // 降落板真实位置（仿真中由Gazebo插件提供）

Detection_result g_landpad_det;     // include/mission_utils.h 中定义的视觉检测结构体

//---------------------------------------Track---------------------------------------------
//                                        .
//                                        .
//                                        .

// ----------------------------控制器计算的相关全局变量------------------------------
//                                        .
//                                        .
//                                        .


// 四种状态机，整个程序中循环执行判断状态和跳转
enum EXEC_STATE {
    WAITING,
    TRACKING_STAGE1,
    TRACKING_STAGE2,
    LOST,
    LANDING,
};
EXEC_STATE exec_state;

bool tracking_stage_change_flag = false;

float g_distance_to_pad;
float g_arm_height_to_ground;
float g_arm_distance_to_pad;


// ---------------------------------- 视觉目标检测回调 ------------------------------------
void landpadDetCb(const prometheus_msgs::DetectionInfo::ConstPtr &msg) {
    g_landpad_det.object_name = "landpad";
    g_landpad_det.Detection_info = *msg;
    // 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    // 相机安装误差 在mission_utils.h中设置场
    // 相机坐标向下，机体系坐标是东北天的设置，x, y轴交换并反向，z轴反向且偏差为负
    // 相机系 -> 机体系
    g_landpad_det.pos_body_frame[0] = -g_landpad_det.Detection_info.position[1] + g_camera_offset[0];
    g_landpad_det.pos_body_frame[1] = -g_landpad_det.Detection_info.position[0] + g_camera_offset[1];
    g_landpad_det.pos_body_frame[2] = -g_landpad_det.Detection_info.position[2] + g_camera_offset[2];

    // 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
    g_landpad_det.pos_body_enu_frame = R_Body_to_ENU * g_landpad_det.pos_body_frame;

    if (g_use_pad_height) { // 设置是true
        //若已知降落板高度，则无需使用深度信息。
        g_landpad_det.pos_body_enu_frame[2] = g_pad_height - g_UAVState.position[2];
    }

    // 机体惯性系 -> 惯性系，目标在惯性系的位置 = 飞机位置 + 目标在机体惯性系的位置
    g_landpad_det.pos_enu_frame[0] = g_UAVState.position[0] + g_landpad_det.pos_body_enu_frame[0];
    g_landpad_det.pos_enu_frame[1] = g_UAVState.position[1] + g_landpad_det.pos_body_enu_frame[1];
    g_landpad_det.pos_enu_frame[2] = g_UAVState.position[2] + g_landpad_det.pos_body_enu_frame[2];
    // 此降落方案不考虑偏航角
    g_landpad_det.att_enu_frame[2] = 0.0;

    // 目标获得和丢失数积累
    if (g_landpad_det.Detection_info.detected) {
        g_landpad_det.num_regain++;
        g_landpad_det.num_lost = 0;
    }
    else {
        g_landpad_det.num_regain = 0;
        g_landpad_det.num_lost++;
    }

    // 目标丢失，标志位变成false
    if (g_landpad_det.num_lost > VISION_THRES) {
        g_landpad_det.is_detected = false;
    }

    // 目标得到，标志变成true
    if (g_landpad_det.num_regain > VISION_THRES) {
        g_landpad_det.is_detected = true;
    }
}

// ---------------------------------- 无人机状态回调 ------------------------------------
void droneStateCb(const prometheus_msgs::DroneState::ConstPtr &msg) {
    g_UAVState = *msg;

    curr_mode = msg->mode;

    R_Body_to_ENU = get_rotation_matrix(g_UAVState.attitude[0], g_UAVState.attitude[1], g_UAVState.attitude[2]);

    Eigen::Matrix3f R_c2B = Eigen::Matrix3f::Zero();
    R_c2B(0,1) = -1;
    R_c2B(1,0) = -1;
    R_c2B(2,2) = -1;

    R_c2v = R_Body_to_ENU * R_c2B;
}


// --------------------------- 主函数中inline读取参数 ------------------------------
inline void readParams(const ros::NodeHandle &nh) {
    nh.param<int>("uav_id", g_uav_id, 1);
    //强制上锁高度
    nh.param<float>("arm_height_to_ground", g_arm_height_to_ground, 0.4);
    //强制上锁距离
    nh.param<float>("arm_distance_to_pad", g_arm_distance_to_pad, 0.3);
    // 是否使用降落板绝对高度
    nh.param<bool>("use_pad_height", g_use_pad_height, false);
    nh.param<float>("pad_height", g_pad_height, 0.01);

    // 目标丢失时，最大到飞行高度，如果高于这个值判定为任务失败
    nh.param<float>("max_height", max_height, 3.0);

    // 相机安装偏移,规定为:相机在机体系(质心原点)的位置
    nh.param<float>("camera_offset_x", g_camera_offset[0], 0.0);
    nh.param<float>("camera_offset_y", g_camera_offset[1], 0.0);
    nh.param<float>("camera_offset_z", g_camera_offset[2], 0.0);
}

inline void topicSub(ros::NodeHandle &nh) {
    //【订阅】降落板与无人机的相对位置及相对偏航角  单位：米   单位：弧度
    //  方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    //  标志位：   detected 用作标志位 true代表识别到目标 false代表丢失目标
    static ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpadDetCb);

    // 【订阅】无人机状态
    static ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, droneStateCb);

    // 【订阅】地面真值，此信息仅做比较使用 不强制要求提供
    static ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, [&](const nav_msgs::Odometry::ConstPtr &msg)
                                                                              { g_GroundTruth = *msg; });
}

static ros::Publisher g_command_pub, plot_pub;

inline void topicAdv(ros::NodeHandle &nh) {
    //【发布】发送给控制模块命令
    // g_command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10);
    g_command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

    //【发布】自定义发布的画图信息
    plot_pub = nh.advertise<visual_servo_landing::MyPlot>("/MyPlot", 10);
}

// >>>>>>>>>>>>>>>>>>>>>>>>>>>> 具体控制器计算 <<<<<<<<<<<<<<<<<<<<<<<<<<<<
void controller(ros::Time start_time, ros::Time last_time) {
    // 记录当前时间
    ros::Time current_time = ros::Time::now();

    // 计算时间间隔，elapsed_time是运行周期，t是时间轴
    ros::Duration elapsed_time = current_time - last_time;
    ros::Duration t = current_time - start_time;
    ss << "t = " << t.toSec() << std::endl;

    // 第一阶段降落满足时间
    ros::Time stable_start;
    ros::Duration stable_duration(10,0);


    // ----------------------- 计算期望控制器速度 v_r --------------------------
    // v_r = Zd*(K_s*L_s.inverse()*ep_s - d_rho_s*xi_s) + hat_vm;
    // ss << RED << "v_r = " << std::endl << TAIL <<  v_r << std::endl;


    // -------------------------- 速度控制指令下发 ---------------------------
    g_command_now.Mode = prometheus_msgs::ControlCommand::Move;
    // g_command_now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
    g_command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;

    // 使用机体惯性系作为误差进行惯性系的速度控制
    g_command_now.Reference_State.velocity_ref[0] = v_r[0];
    g_command_now.Reference_State.velocity_ref[1] = v_r[1]; 
    g_command_now.Reference_State.velocity_ref[2] = v_r[2];
    g_command_now.Reference_State.Yaw_Rate_Mode = true;
    g_command_now.Reference_State.yaw_rate_ref = 0;

    // 输出变量，作图
    for(int i = 0; i < 3; i++) {
        plot_msg.position[i] = g_UAVState.position[i];
        plot_msg.rho_s[i] = rho_s(i,i);
        plot_msg.s_ohr[i] = -rho_s(i,i);
        plot_msg.s[i] = s[i];
        plot_msg.e_s[i] = e_s[i];
        plot_msg.hat_vm[i] = hat_vm(i);
        plot_msg.v_r[i] = v_r(i);
    }
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 主函数 <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv) {
    ros::init(argc, argv, "my_landing");
    ros::NodeHandle nh("~");

    // 节点运行频率： 30hz
    ros::Rate rate(30.0);

    // 记录时间
    ros::Time stage1_start_time;
    ros::Time stage2_start_time;
    ros::Time last_time;

    // --------------------- 相机内参（也可以读其他yaml文件）-----------------
    Eigen::Matrix3f camera_matrix = Eigen::Matrix3f::Zero();
    float fx = 816.80845;
    float fy = 818.41599;
    float cx = 311.44342;
    float cy = 214.47836;
    camera_matrix(0, 0) = fx;
    camera_matrix(0, 2) = cx;
    camera_matrix(1, 1) = fy;
    camera_matrix(1, 2) = cy;
    camera_matrix(2, 2) = 1.0f;

    // 读取配置参数
    readParams(nh);
    // 订阅话题
    topicSub(nh);
    // 发布话题
    topicAdv(nh);

    g_command_now.Command_ID = 0;
    exec_state = EXEC_STATE::WAITING;
    while (ros::ok()) {
        ros::spinOnce();
        
        // 持续更新指令ID，不然无法进入OFFBOARD模式
        g_command_now.Command_ID = g_command_now.Command_ID + 1;
        g_command_now.header.stamp = ros::Time::now();
        g_command_now.source = "visual_servo_landing";

        // --------------- 未进入OFFBOARD模式，等待 ---------------
        if (curr_mode != "OFFBOARD") {
            exec_state = EXEC_STATE::WAITING;
            ss << "current mode: " << RED << curr_mode << endl
                << BLUE << "Waiting for enter OFFBOARD state \n";
            // PCOUT(1, TAIL, "Waiting for enter OFFBOARD state");
            // continue;
        }

        switch (exec_state) {
            // --------------------- 1. 初始状态，等待视觉检测结果 ----------------------
            case WAITING: {
                // 检测到landingpad就开启降落TRACKING
                if (g_landpad_det.is_detected && curr_mode == "OFFBOARD") {
                    exec_state = TRACKING_STAGE1;

                    // 记录当前时间，开始程序
                    stage1_start_time = ros::Time::now();
                    last_time = ros::Time::now();

                    PCOUT(0, BLUE, "Time to begin.");
                    break;
                }

                // 默认高度为2米，Modules/uav_control/launch/uav_control_outdoor.yaml
                g_command_now.Mode = prometheus_msgs::ControlCommand::Hold;
                ss << GREEN << "Waiting for the detection result. \n";
                // PCOUT(1, GREEN, "Waiting for the detection result.");
                break;
            }

            // --------------------------- 2. 外层二维码追踪状态 ---------------------------
            case TRACKING_STAGE1: {
                // 丢失,进入LOST状态
                if (!g_landpad_det.is_detected) {
                    exec_state = LOST;
                    PCOUT(0, YELLOW, "Lost the Landing Pad.");
                    break;
                }

                // 当前阶段的【主要控制变量的期望值】 & 【控制量计算】
                controller(stage1_start_time, last_time);

                // 更新时间
                last_time = ros::Time::now();
                stage2_start_time = ros::Time::now();

                // PCOUT(0, RED, "运行到这里了吗.");
                break;
            }
            // --------------------------- 3. 内层二维码追踪状态 --------------------------
            case TRACKING_STAGE2: {
                // 丢失,进入LOST状态
                if (!g_landpad_det.is_detected) {
                    exec_state = LOST;
                    PCOUT(0, YELLOW, "Lost the Landing Pad.");
                    break;
                }

                // 当前阶段的【主要控制变量的期望值】 & 【控制量计算】
                controller(stage2_start_time, last_time);

                // 更新时间
                last_time = ros::Time::now();

                // PCOUT(0, RED, "运行到这里了吗.");
                break;
            }

            // ------------------------ 4. 目标丢失后的任务状态 ------------------------
            // 降落任务失败，原点进入降落模式。
            case LOST: {
                static int lost_time = 0;
                lost_time++;

                PCOUT(0, YELLOW, "Target Lost.");

                if (lost_time < 10.0) {
                    // 原地等待的动作

                    ros::Duration(0.4).sleep();
                }
                else {
                    // 等待超时后的动作

                    // 如果上升超过原始高度，则认为任务失败，则直接降落
                    if (g_UAVState.position[2] >= max_height) {
                        exec_state = LANDING;
                        lost_time = 0;
                        PCOUT(0, RED, "Mission failed, landing... ");
                    }
                }

                // 重新获得目标信息，进入TRACKING
                if (g_landpad_det.is_detected) {
                    exec_state = TRACKING_STAGE1;
                    lost_time = 0;
                    PCOUT(0, GREEN, "Regain the Landing Pad.");
                }

                break;
            }

            // ------------------------------ 5. 降落状态 -----------------------------
            case LANDING: {
                g_command_now.Mode = prometheus_msgs::ControlCommand::Land;
                break;
            }
        }

        // --------------- 控制和作图信息话题发布 & 状态输出显示 ------------------
        g_command_pub.publish(g_command_now);
        plot_pub.publish(plot_msg);

        PCOUT(0.2, TAIL, ss.str());
        ss.str(" ");

        // 按照循环频率延时
        rate.sleep();
    }

    return 0;
}